#if defined(MODBUS_TCP) || defined(MODBUS_RTU)
/*
 * Copyright (c) 2001,2016 Mario de Sousa (msousa@fe.up.pt)
 *
 * This file is part of the Modbus library for Beremiz and matiec.
 *
 * This Modbus library is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser
 * General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this Modbus library.  If not, see <http://www.gnu.org/licenses/>.
 *
 * This code is made available on the understanding that it will not be
 * used in safety-critical situations without a full and competent review.
 */

/* mb_slave.c */

#include <fcntl.h> /* File control definitions */

#ifdef ARDUINO
#elif defined(windows_x86)

#include <windows.h>

#endif
#ifdef __linux__
#include <termio.h>     /* POSIX terminal control definitions */
#include <netinet/in.h> /* required for htons() and ntohs() */
#endif

#include <logger_rte.h>
#include <plc_modbus.h>
#include "mb_layer1.h"
#include "mb_slave.h"
#include "mb_slave_private.h"
#include "hwboard.h"


#define modbus_write fptr_[layer1_fin].modbus_write
#define modbus_read fptr_[layer1_fin].modbus_read
#define modbus_init fptr_[layer1_fin].modbus_init
#define modbus_done fptr_[layer1_fin].modbus_done
#define modbus_connect fptr_[layer1_fin].modbus_connect
#define modbus_listen fptr_[layer1_fin].modbus_listen
#define modbus_close fptr_[layer1_fin].modbus_close
#define modbus_silence_init fptr_[layer1_fin].modbus_silence_init
#define modbus_get_min_timeout fptr_[layer1_fin].modbus_get_min_timeout

/* the lower two bits of ttyfd are used to store the index to layer1 function pointers */
/* layer1_fin index to fptr_[] is in lowest 2 bits of fd */
#define get_ttyfd()                                              \
int layer1_fin = node->node_address.naf;                               \
    int ttyfd = node->node_address.naf;                          \
    if (node->fd < 0)                                            \
    {                                                            \
        ttyfd = node->fd;                                        \
        layer1_fin = node->node_address.naf; /* use modbusTCP */ \
    }

/******************************************/
/******************************************/
/**                                      **/
/**         Global Variables...          **/
/**                                      **/
/******************************************/
/******************************************/
/* The layer 1 (RTU, ASCII, TCP) implementations will be adding some
 *  header and tail bytes (e.g. CRC) to the packet we build here. Since
 *  layer1 will re-use the same buffer allocated in this slave layer
 *  (so as not to continuosly copy the same info from buffer to buffer),
 *  we need to allocate more bytes than those strictly required for this
 *  slave layer. Therefore, the extra_bytes parameter.
 *
 *  Note that we add one more extra byte to the response buffer.
 *  This is because some response packets will not be starting off
 *  at byte 0, but rather at byte 1 of the buffer. This is in order
 *  to guarantee that the data that is sent on the buffer is aligned
 *  on even bytes (the 16 bit words!). This will allow the application
 *  (layer above the one implemented in this file - i.e. the callback
 *  functions) to reference this memory as an u16 *, without producing
 *  'bus error' messages in some embedded devices that do not allow
 *   acessing u16 on odd numbered addresses.
 */
static int buff_extra_bytes_;
#define RESP_BUFFER_SIZE (MAX_L2_FRAME_LENGTH + buff_extra_bytes_ + 1)

/******************************************/
/******************************************/
/**                                      **/
/**       Local Utility functions...     **/
/**                                      **/
/******************************************/
/******************************************/

/*
 * Function to determine next transaction id.
 *
 * We use a library wide transaction id, which means that we
 * use a new transaction id no matter what slave to which we will
 * be sending the request...
 */
static inline u16 next_transaction_id() {
    static u16 next_id = 0;
    return next_id++;
}

/*
 * Functions to convert u16 variables
 * between network and host byte order
 *
 * NOTE: Modbus uses MSByte first, just like
 *       tcp/ip, so we could be tempted to use the htons() and
 *       ntohs() functions to guarantee code portability.
 *
 *       However, on some embedded systems running Linux
 *       these functions only work if the 16 bit words are
 *       stored on even addresses. This is not always the
 *       case in our code, so we have to define our own
 *       conversion functions...
 */

/* if using gcc, use it to determine byte order... */
#ifndef __BYTE_ORDER
#if defined(__GNUC__)
/* We have GCC, which should define __LITTLE_ENDIAN__ */
#if defined(__LITTLE_ENDIAN__)
#define __BYTE_ORDER __LITTLE_ENDIAN
#else
#define __BYTE_ORDER __BIG_ENDIAN
#endif
#endif /* __GNUC__ */
#endif /* __BYTE_ORDER */

/* If we still don't know byte order, try to get it from <sys/param.h> */
#ifndef __BYTE_ORDER
#include <sys/param.h>
#endif

#ifndef __BYTE_ORDER
#ifdef BYTE_ORDER
#if BYTE_ORDER == LITTLE_ENDIAN
#define __BYTE_ORDER __LITTLE_ENDIAN
#else
#if BYTE_ORDER == BIG_ENDIAN
#define __BYTE_ORDER __BIG_ENDIAN
#endif
#endif
#endif /* BYTE_ORDER */
#endif /* __BYTE_ORDER */

#ifdef __BYTE_ORDER
#if __BYTE_ORDER == __LITTLE_ENDIAN

/**************************************************************/
/* u16 conversion functions to use on little endian platforms */
/**************************************************************/

static inline u16 mb_hton(u16 w) {
    u16 tmp;
    tmp = (w & 0x00FF);
    tmp = (u16) (((w & 0xFF00) >> 0x08) | (tmp << 0x08));
    return tmp;
}

#define mb_ntoh(a) mb_hton(a)

static inline void mb_hton_count(u16 *w, int count) {
    for (int i = 0; i < count; i++) {
        /* swap the bytes around...
         *  a = a ^ b;
         *  b = a ^ b;
         *  a = a ^ b;
         */
        ((u8 *) (w + i))[0] ^= ((u8 *) (w + i))[1];
        ((u8 *) (w + i))[1] ^= ((u8 *) (w + i))[0];
        ((u8 *) (w + i))[0] ^= ((u8 *) (w + i))[1];
    }
}

#define mb_ntoh_count(w, count) mb_hton_count(w, count)

#else
#if __BYTE_ORDER == __BIG_ENDIAN
/***********************************************************/
/* u16 conversion functions to use on big endian platforms */
/***********************************************************/

/* We do not need to swap the bytes around!  */
#define mb_ntoh(val) (val)
#define mb_hton(val) (val)
#define mb_hton_count(w, count) /* empty ! */
#define mb_ntoh_count(w, count) /* empty ! */

#else

/********************************************************/
/* u16 conversion functions to use on generic platforms */
/********************************************************/

/* We don't know the byte order, so we revert to the
 * standard htons() and ntohs() ...
 */
static inline u16 mb_hton(u16 h_value)
{
    return htons(h_value); /* return h_value; */
}

static inline u16 mb_ntoh(u16 m_value)
{
    return ntohs(m_value); /* return m_value; */
}

static inline void mb_hton_count(u16 *w, int count)
{
    int i;
    for (i = 0; i < count; i++)
    {
        w[i] = mb_hton(w[i]);
    }
}

static inline void mb_ntoh_count(u16 *w, int count)
{
    int i;
    for (i = 0; i < count; i++)
    {
        w[i] = mb_ntoh(w[i]);
    }
}

#endif
#endif
#endif /* __BYTE_ORDER */

/* Safe versions of the conversion functions!
 *
 * Note that these functions always work, whatever the endiannes
 * of the machine that executes it!
 *
 * It is also safe because the resulting value may be stored
 * on an odd address even on machines that do not allow directly
 * accessing u16 bit words on odd addresses.
 */
static inline int mb_hton_safe(u16 from, u16 *to_ptr) {
    ((u8 *) to_ptr)[1] = (from & 0x00FF);
    ((u8 *) to_ptr)[0] = ((from & 0xFF00) >> 0x08);
    return 0;
}

#define mb_ntoh_safe(a, b) mb_hton_safe(a, b)

/*  return Most Significant Byte of value; */
static inline u8 msb(u16 value) {
    return (value >> 8) & 0xFF;
}

/*  return Least Significant Byte of value; */
static inline u8 lsb(u16 value) {
    return value & 0xFF;
}

#define u16_v(char_ptr) (*((u16 *)(&(char_ptr))))

/***********************************************/
/***********************************************/
/**                                           **/
/**    Handle requests from master/client     **/
/**                                           **/
/***********************************************/
/***********************************************/

/* Handle functions 0x01 and 0x02 */
typedef int (*read_bits_callback_t)(memarea *arg, u16 start_addr, u16 bit_count, u8 *data_bytes);

static int handle_read_bits(u8 *query_packet,
                            u8 **resp_packet_ptr,
                            u8 *error_code,
                            read_bits_callback_t read_bits_callback,
                            memarea *callback_arg) {
    u16 start_addr;
    u16 count;
    int res;
    u8 *resp_packet;
    auto *area = callback_arg;
    /* If no callback, handle as if function is not supported... */
    if (read_bits_callback == nullptr) {
        *error_code = ERR_ILLEGAL_FUNCTION;
        return -1;
    }

    /* in oprder for the data in this packet to be aligned on even numbered addresses, this
     *  response packet will start off at an odd numbered byte...
     *  We therefore add 1 to the address where the packet starts.
     */
    (*resp_packet_ptr)++;
    resp_packet = *resp_packet_ptr;

    /* NOTE:
     *  Modbus uses high level addressing starting off from 1, but
     *  this is sent as 0 on the wire!
     *  We could expect the user to specify high level addressing
     *   starting at 1, and do the conversion to start off at 0 here.
     *   However, to do this we would then need to use an uint32_t data type
     *   to correctly hold the address supplied by the user (which could
     *   correctly be 65536, which does not fit in an u16), which would
     *   in turn require us to check whether the address supplied by the user
     *   is correct (i.e. <= 65536).
     *  I decided to go with the other option of using an u16, and
     *   requiring the user to use addressing starting off at 0!
     */
    /* start_addr = mb_ntoh(u16_v(query_packet[2])) + 1; */
    mb_ntoh_safe(u16_v(query_packet[2]), &start_addr);
    mb_ntoh_safe(u16_v(query_packet[4]), &count);

#ifdef DEBUG
    // logger.debug("handle_read_input_bits() called. slave=%d, function=%d, start_addr=%d, count=%d\n",
    //        query_packet[0], query_packet[1], start_addr, count);
#endif

    if ((count > MAX_READ_BITS) || (count < 1)) {
        *error_code = ERR_ILLEGAL_DATA_VALUE;
        return -1;
    }

    /* start building response frame... */
    resp_packet[0] = query_packet[0];       /* slave */
    resp_packet[1] = query_packet[1];       /* function (either 0x01 or 0x02 ! */
    resp_packet[2] = (u8) ((count + 7) / 8); /* number of data bytes = ceil(count/8) */
    if (start_addr >= 60000 && board.modbus_cb_read_bits(start_addr, count, &(resp_packet[3])) == 0) {
        // mb_hton_count((u16 *)&(resp_packet[3]), count);
        return resp_packet[2] + 3;
    }

    /* Remember, we are using addressing starting off at 0, in the start_addr variable! */
    /*  This means that he highest acceptable address is 65535, when count=1 .... */
    /* Note the use of 65536 in the comparison will force automatic upgrade of u16 variables! */
    /*    => start_addr + count will nver overflow the u16 type!                              */
    if ((start_addr < area->start_address) || (start_addr + count > (area->start_address + area->length))) {
        *error_code = ERR_ILLEGAL_DATA_ADDRESS;
        return -1;
    }
    res = read_bits_callback(callback_arg, start_addr - area->start_address, count, &(resp_packet[3]));
    if (res == -2) {
        *error_code = ERR_ILLEGAL_DATA_ADDRESS;
        return -1;
    }
    if (res < 0) {
        *error_code = ERR_SLAVE_DEVICE_FAILURE;
        return -1;
    }

    return resp_packet[2] + 3; /* packet size is data length + 3 bytes -> slave, function, count */
}

/* Handle function 0x01 */
int
handle_read_output_bits(u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_rwbits_t *callbacks) {
    memarea *area = callbacks->arg;
    for (int i = 0; area->buf != nullptr; i++, area = &callbacks->arg[i]) {
        int ret = handle_read_bits(query_packet, resp_packet_ptr, error_code, callbacks->read_outbits, area);
        if (ret > 0)
            return ret;
    }
    return -ERR_ILLEGAL_DATA_ADDRESS;
}

/* Handle function 0x02 */
int
handle_read_input_bits(u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_robits_t *callbacks) {
    memarea *area = callbacks->arg;
    for (int i = 0; area->buf != nullptr; i++, area = &callbacks->arg[i]) {

        int ret = handle_read_bits(query_packet, resp_packet_ptr, error_code, callbacks->read_inbits, area);
        if (ret > 0)
            return ret;
    }
    return -ERR_ILLEGAL_DATA_ADDRESS;
}

/* Handle functions 0x03 and 0x04 */
typedef int (*read_words_callback_t)(memarea *arg, u16 start_addr, u16 word_count, u16 *data_words);

static int handle_read_words(u8 *query_packet,
                             u8 **resp_packet_ptr,
                             u8 *error_code,
                             read_words_callback_t read_words_callback,
                             memarea *callback_arg) {
    u16 start_addr;
    u16 count;
    int res;
    u8 *resp_packet;

    /* If no callback, handle as if function is not supported... */
    if (read_words_callback == nullptr) {
        *error_code = ERR_ILLEGAL_FUNCTION;
        return -1;
    }

    /* See equivalent comment in handle_read_bits() */
    (*resp_packet_ptr)++;
    resp_packet = *resp_packet_ptr;

    /* See equivalent comment in handle_read_bits() */
    mb_ntoh_safe(u16_v(query_packet[2]), &start_addr);
    mb_ntoh_safe(u16_v(query_packet[4]), &count);

#ifdef DEBUG
    logger.debug("handle_read_output_words() called. slave=%d, function=%d, start_addr=%d, count=%d\n",
                 query_packet[0], query_packet[1], start_addr, count);
#endif

    if ((count > MAX_READ_REGS) || (count < 1)) {
        *error_code = ERR_ILLEGAL_DATA_VALUE;
        return -1;
    }

    /* See equivalent comment in handle_read_bits() */
    if (start_addr + count > 65536) {
        *error_code = ERR_ILLEGAL_DATA_ADDRESS;
        return -1;
    }

    /* start building response frame... */
    resp_packet[0] = query_packet[0]; /* slave     */
    resp_packet[1] = query_packet[1]; /* function code, either 0x03 or 0x04 !!!*/
    resp_packet[2] = (u8) (count * 2); /* number of bytes of data... */
    if (start_addr >= 60000 && board.modbus_cb_read(start_addr, count, (u16 *) &(resp_packet[3])) == 0) {
        mb_hton_count((u16 *) &(resp_packet[3]), count);
        return resp_packet[2] + 3;
    }
    res = read_words_callback(callback_arg, start_addr, count, (u16 *) &(resp_packet[3]));
    if (res == -2) {
        *error_code = ERR_ILLEGAL_DATA_ADDRESS;
        return -1;
    }
    if (res < 0) {
        *error_code = ERR_SLAVE_DEVICE_FAILURE;
        return -1;
    }
    *error_code = 0;

    /* convert all data from host to network byte order. */
    mb_hton_count((u16 *) &(resp_packet[3]), count);

    return resp_packet[2] + 3; /* packet size is data length + 3 bytes -> slave, function, count */
}

/* Handle function 0x03 */
int
handle_read_output_words(u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_words_t *callbacks) {
    int ret;
    memarea *area = callbacks->arg;

    for (int i = 0; area->buf != nullptr; i++, area = &callbacks->arg[i]) {
        ret = handle_read_words(query_packet, resp_packet_ptr, error_code, callbacks->read_outwords, area);
        if (ret > 0)
            return ret;
    }
    return -ERR_ILLEGAL_DATA_ADDRESS;
}

/* Handle function 0x04 */
int handle_read_input_words(u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code,
                            mb_slave_callback_rowords_t *callbacks) {
    memarea *area = callbacks->arg;
    for (int i = 0; area->buf != nullptr; i++, area = &callbacks->arg[i]) {
        int ret = handle_read_words(query_packet, resp_packet_ptr, error_code, callbacks->read_inwords, area);
        if (ret > 0)
            return ret;
    }
    return -ERR_ILLEGAL_DATA_ADDRESS;
}

/* Handle function 0x05 */
int
handle_write_output_bit(u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_rwbits_t *callbacks,
                        void *node) {
    u16 start_addr;
    int res;
    u8 *resp_packet;

    /* If no callback, handle as if function is not supported... */
    if (callbacks->write_outbits == nullptr) {
        *error_code = ERR_ILLEGAL_FUNCTION;
        return -1;
    }

    resp_packet = *resp_packet_ptr;

    /* See equivalent comment in handle_read_bits() */
    mb_ntoh_safe(u16_v(query_packet[2]), &start_addr);

#ifdef DEBUG
    logger.debug("handle_write_output_bit() called. slave=%d, function=%d, start_addr=%d\n",
                 query_packet[0], query_packet[1], start_addr);
#endif

    // byte 5 Must be 0x00, byte 4 must be 0x00 or 0xFF !!
    if ((query_packet[5] != 0) ||
        ((query_packet[4] != 0) && (query_packet[4] != 0xFF))) {
        *error_code = ERR_ILLEGAL_DATA_VALUE;
        return -1;
    }

    /* Address will always be valid, no need to check! */
    // if (start_addr > 65535) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}

    /* start building response frame... */
    resp_packet[0] = query_packet[0]; /* slave */
    resp_packet[1] = query_packet[1]; /* function */
    resp_packet[2] = query_packet[2]; /* start address - hi byte */
    resp_packet[3] = query_packet[3]; /* start address - lo byte */
    resp_packet[4] = query_packet[4]; /* value: 0x00 or 0xFF */
    resp_packet[5] = query_packet[5]; /* value: must be 0x00 */
    if (start_addr >= 60000 && board.modbus_cb_write_bit(start_addr, 1, query_packet[4]))
        return 6;
    memarea *area = callbacks->arg;
    for (int i = 0; area->buf != nullptr; i++, area = &callbacks->arg[i]) {
        if ((start_addr < area->start_address) || ((start_addr + 1) > (area->start_address + area->length)))
            continue; /* ERR_ILLEGAL_DATA_ADDRESS defined in mb_util.h */

        res = (callbacks->write_outbits)(area, start_addr - area->start_address, 1, &(query_packet[4]), node);
        if (!res)
            break;
        if (res == -2) {
            *error_code = ERR_ILLEGAL_DATA_ADDRESS;
            continue;
        }
        if (res < 0) {
            *error_code = ERR_SLAVE_DEVICE_FAILURE;
            continue;
        }
    }
    return 6; /* response packet size, including slave id in byte 0 */
}

/* Handle function 0x06 */
int
handle_write_output_word(u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_words_t *callbacks,
                         void *node) {
    u16 start_addr;
    int res;
    u8 *resp_packet;

    /* If no callback, handle as if function is not supported... */
    if (callbacks->write_outwords == nullptr) {
        *error_code = ERR_ILLEGAL_FUNCTION;
        return -1;
    }

    resp_packet = *resp_packet_ptr;

    /* See equivalent comment in handle_read_bits() */
    mb_ntoh_safe(u16_v(query_packet[2]), &start_addr);

#ifdef DEBUG
    logger.debug("handle_write_output_word() called. slave=%d, function=%d, start_addr=%d\n",
                 query_packet[0], query_packet[1], start_addr);
#endif

    /* Address will always be valid, no need to check! */
    // if (start_addr > 65535) {*error_code = ERR_ILLEGAL_DATA_ADDRESS; return -1;}

    /* start building response frame... */
    resp_packet[0] = query_packet[0]; /* slave           */
    resp_packet[1] = query_packet[1]; /* function        */
    resp_packet[2] = query_packet[2]; /* start address - hi byte */
    resp_packet[3] = query_packet[3]; /* start address - lo byte */
    resp_packet[4] = query_packet[4]; /* value - hi byte */
    resp_packet[5] = query_packet[5]; /* value - lo byte */

    /* convert data from network to host byte order */
    mb_ntoh_count((u16 *) &(query_packet[4]), 1);
    memarea *area = callbacks->arg;
    if (start_addr >= 60000 && board.modbus_cb_write(start_addr, 1, (u16 *) &(query_packet[4])))
        return 6;
    for (int i = 0; area->buf != nullptr; i++, area = &callbacks->arg[i]) {
        res = (callbacks->write_outwords)(area, start_addr, 1, (u16 *) &(query_packet[4]), node);
        if (!res)
            break;
        if (res == -2) {
            *error_code = ERR_ILLEGAL_DATA_ADDRESS;
            continue;
        }
        if (res < 0) {
            *error_code = ERR_SLAVE_DEVICE_FAILURE;
            continue;
        }
    }
    return 6; /* packet size is 6 -> slave, function, addr(2), value(2) */
}

/* Handle function 0x0F */
int
handle_write_output_bits(u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_rwbits_t *callbacks,
                         void *node) {
    u16 start_addr;
    u16 count;
    int res;
    u8 *resp_packet;

    /* If no callback, handle as if function is not supported... */
    if (callbacks->write_outbits == nullptr) {
        *error_code = ERR_ILLEGAL_FUNCTION;
        return -1;
    }

    resp_packet = *resp_packet_ptr;

    /* See equivalent comment in handle_read_bits() */
    mb_ntoh_safe(u16_v(query_packet[2]), &start_addr);
    mb_ntoh_safe(u16_v(query_packet[4]), &count);

#ifdef DEBUG
    // logger.debug("handle_write_output_bits() called. slave=%d, function=%d, start_addr=%d, count=%d\n",
    //              query_packet[0], query_packet[1], start_addr, count);
#endif

    if ((count > MAX_WRITE_COILS) || (count < 1) || ((count + 7) / 8 != query_packet[6])) {
        *error_code = ERR_ILLEGAL_DATA_VALUE;
        return -1;
    }

    /* See equivalent comment in handle_read_bits() */
    if (start_addr + count > 65536) {
        *error_code = ERR_ILLEGAL_DATA_ADDRESS;
        return -1;
    }

    /* start building response frame... */
    resp_packet[0] = query_packet[0]; /* slave */
    resp_packet[1] = query_packet[1]; /* function */
    resp_packet[2] = query_packet[2]; /* start address - hi byte */
    resp_packet[3] = query_packet[3]; /* start address - lo byte */
    resp_packet[4] = query_packet[4]; /* count - hi byte */
    resp_packet[5] = query_packet[5]; /* count - lo byte */
    if (start_addr >= 60000 && board.modbus_cb_write_bits(start_addr, count, &(query_packet[7])))
        return 6;
    memarea *area = callbacks->arg;
    for (int i = 0; area->buf != nullptr; i++, area = &callbacks->arg[i]) {
        if ((start_addr < area->start_address) || ((start_addr + count) > (area->start_address + area->length)))
            continue; /* ERR_ILLEGAL_DATA_ADDRESS defined in mb_util.h */

        res = (callbacks->write_outbits)(area, start_addr - area->start_address, count, &(query_packet[7]), node);
        if (!res)
            break;
        if (res == -2) {
            *error_code = ERR_ILLEGAL_DATA_ADDRESS;
            continue;
        }
        if (res < 0) {
            *error_code = ERR_SLAVE_DEVICE_FAILURE;
            continue;
        }
    }
    return 6; /* packet size is 6 -> slave, function, addr(2), count(2) */
}

/* Handle function 0x10 */
int
handle_write_output_words(u8 *query_packet, u8 **resp_packet_ptr, u8 *error_code, mb_slave_callback_words_t *callbacks,
                          void *node) {
    u16 start_addr;
    u16 count;
    int res;
    u8 *resp_packet;

    /* If no callback, handle as if function is not supported... */
    if (callbacks->write_outwords == nullptr) {
        *error_code = ERR_ILLEGAL_FUNCTION;
        return -1;
    }

    resp_packet = *resp_packet_ptr;

    /* See equivalent comment in handle_read_bits() */
    mb_ntoh_safe(u16_v(query_packet[2]), &start_addr);
    mb_ntoh_safe(u16_v(query_packet[4]), &count);

    if ((count > MAX_WRITE_REGS) || (count < 1) || (count * 2 != query_packet[6])) {
        *error_code = ERR_ILLEGAL_DATA_VALUE;
        return -1;
    }

    /* See equivalent comment in handle_read_bits() */
    if (start_addr + count > 65536) {
        *error_code = ERR_ILLEGAL_DATA_ADDRESS;
        return -1;
    }

    /* start building response frame... */
    resp_packet[0] = query_packet[0]; /* slave           */
    resp_packet[1] = query_packet[1]; /* function        */
    resp_packet[2] = query_packet[2]; /* start address - hi byte */
    resp_packet[3] = query_packet[3]; /* start address - lo byte */
    resp_packet[4] = query_packet[4]; /* count - hi byte */
    resp_packet[5] = query_packet[5]; /* count - lo byte */

    /* convert all data from network to host byte order */
    mb_ntoh_count((u16 *) &(query_packet[7]), count);
    memarea *area = callbacks->arg;
    if (start_addr >= 60000 && board.modbus_cb_write(start_addr, count, (u16 *) &(query_packet[7])))
        return 6;
    for (int i = 0; area->buf != nullptr; i++, area = &callbacks->arg[i]) {
        res = (callbacks->write_outwords)(area, start_addr, count, (u16 *) &(query_packet[7]), node);
        if (!res)
            break;
        if (res == -2) {
            *error_code = ERR_ILLEGAL_DATA_ADDRESS;
            continue;
        }
        if (res < 0) {
            *error_code = ERR_SLAVE_DEVICE_FAILURE;
            continue;
        }
    }
    return 6; /* packet size is 6 -> slave, function, addr(2), count(2) */
}

/***********************************************/
/***********************************************/
/**                                           **/
/**    initialise / shutdown the library      **/
/**                                           **/
/***********************************************/
/***********************************************/

int mb_slave_init__(int extra_bytes) {
    buff_extra_bytes_ = extra_bytes;
    return 0;
}

int mb_slave_done__() {
    return 0;
}

#if 0
int mb_slave_init(int nd_count) {
  int extra_bytes;

#ifdef DEBUG
  fprintf( stderr, "mb_slave_init()\n");
  fprintf( stderr, "creating %d nodes\n", nd_count);
#endif

  /* initialise layer 1 library */
  if (modbus_init(nd_count, DEF_OPTIMIZATION, &extra_bytes) < 0)
    goto error_exit_0;

  /* initialise this library */
  if (mb_slave_init__(extra_bytes) < 0)
    goto error_exit_1;

  return 0;

error_exit_1:
  modbus_done();
error_exit_0:
  return -1;
}


int mb_slave_done(void) {
  mb_slave_done__(void)
  return modbus_done();
}
#endif

/***********************************************/
/***********************************************/
/**                                           **/
/**        open/close slave connection        **/
/**                                           **/
/***********************************************/
/***********************************************/

/* Create a new slave/server */
/* NOTE: We use the lower 2 bits of the returned node id to identify which
 *       layer1 implementation to use.
 *           0 -> TCP
 *           1 -> RTU
 *           2 -> ASCII
 *           4 -> unused
 *       The node id used by the layer1 is shifted left 2 bits
 *       before returning the node id to the caller!
 */
void *mb_slave_new(server_node_t *node) {
    void *res;
#ifdef DEBUG
    fprintf(stderr, "mb_slave_connect()\n");
#endif

    /* call layer 1 library */
    switch (node->node_address.naf) {
#ifdef MODBUS_TCP
        case naf_tcp:
            res = modbus_tcp_listen(&node->node_address);
            break;
#endif
#ifdef MODBUS_RTU
        case naf_rtu:
            res = modbus_rtu_listen(&node->node_address);
            break;
#endif
#ifdef MODBUS_ASCII
            case naf_ascii:
                res = modbus_ascii_listen(node->node_address);
                break;
#endif
        default:
            logger.error("naf %d", node->node_address.naf);
    }

    return res;
}

int mb_slave_close(server_node_t *node) {
// #ifdef DEBUG
    fprintf(stderr, "mb_slave_close(): nd = %s\n", node->location);
// #endif
    get_ttyfd() /* declare the ttyfd variable!! */
    /* call layer 1 library */
    /* will call one of modbus_tcp_close(), modbus_rtu_close(), modbus_ascii_close() */
    return modbus_close(ttyfd);
}

/***********************************************/
/***********************************************/
/**                                           **/
/**               Run the slave               **/
/**                                           **/
/***********************************************/
/***********************************************/

/* Execute infinite loop waiting and replying to requests coming from clients/master
 * This function enters an infinite loop wating for new connection requests,
 * and for modbus requests over previoulsy open connections...
 *
 * The frames are read from:
 *   -  the node descriptor nd, if nd >= 0
 *       When using TCP, if the referenced node nd was created to listen for new connections
 *       [mb_slave_listen()], then this function will also reply to Modbus data requests arriving
 *       on other nodes that were created as a consequence of accepting connections requests to
 *       the referenced node nd.
 *       All other nodes are ignored!
 *
 *   -  any valid and initialised TCP node descriptor, if nd = -1
 *      In this case, will also accept connection requests arriving from a previously
 *       created node to listen for new connection requests [mb_slave_listen() ].
 *      NOTE: (only avaliable if using TCP)
 *
 * slaveid identifies the address (RTU and ASCII) or slaveid (TCP) that we implement.
 *     Any requests that we receive sent with a slaveid different
 *     than the one specified, and also different to 0, will be silently ignored!
 *     Whatever the slaveid specified, we always reply to requests
 *     to slaveid 0 (the modbus broadcast address).
 *     Calling this function with a slaveid of 0 means to ignore this
 *     parameter and to reply to all requests (whatever the slaveid
 *     used in the request). This should mostly be used by TCP servers...
 */

int mb_slave_run(mb_slave_callback_robits_t *callback_functions_robits,
                 mb_slave_callback_rwbits_t *callback_functions_rwbits,
                 mb_slave_callback_rowords_t *callback_functions_rowords,
                 mb_slave_callback_words_t *callback_functions_words, const u8 *slaveid, const int *shut_down,
                 server_node_t *node) {
    int byte_count;
    u16 transaction_id;
    u8 function;
    u8 error_code = 0;
    int resp_length;
    u8 *query_packet = nullptr;
    u8 *resp_packet;
    u8 resp_buffer_[RESP_BUFFER_SIZE];
    u8 slave;
    void *entry;
    get_ttyfd() /* declare the ttyfd variable!! */

#ifdef DEBUG
    // fprintf(stderr, " mb_slave_run(): Called... fd=%d\n", ttyfd);
#endif

    /* will call one of modbus_tcp_read(), modbus_rtu_read(), modbus_ascii_read() */
    do {
        if (!node->nd) {
            node->nd = mb_slave_new(node);
        }
        entry = node->nd;
        byte_count = modbus_read(&entry,                  /* node descriptor          */
                                 &query_packet,             /* u8 **recv_data_ptr,      */
                                 &transaction_id,           /* u16 *transaction_id,     */
                                 nullptr,                   /* const u8 *send_data,     */
                                 0,                         /* int send_length,         */
                                 2 /* wait indefenitely */ /* const struct timespec *recv_timeout); */
                ,
                                 0);
        if (node->init_state == 0xaa){
            logger.error(" mb_slave_run() node->init_state = 0xaa");
            return -1;
        }
    } while (byte_count <= 2);

#ifdef DEBUG
    // { /* display the hex code of each character received */
    //     int i;
    //     logger.debug(" mb_slave_run() received %d bytes (ptr=%p): \n", byte_count, query_packet);
    //     for (i = 0; i < byte_count; i++)
    //         logger.debug("<0x%2X>", query_packet[i]);
    //     logger.debug("\n");
    // }
#endif

    slave = query_packet[0];
    function = query_packet[1];

    /* We only reply if:
     *       - request was sent to broadcast address   (slave   == 0)
     *  OR   - we were asked to reply to every request (slaveid == 0)
     *  OR   - request matches the slaveid we were asked to accept (slave == slaveid)
     *
     * Otherwise, silently ignore the received request!!!
     */
    if ((*slaveid == 0) || (slave == 0) || (slave == *slaveid)) {
        resp_packet = resp_buffer_;

        switch (function) {
            case 0x01:
                resp_length = handle_read_output_bits(query_packet, &resp_packet, &error_code,
                                                      callback_functions_rwbits);
                break;
            case 0x02:
                resp_length = handle_read_input_bits(query_packet, &resp_packet, &error_code,
                                                     callback_functions_robits);
                break;
            case 0x03:
                resp_length = handle_read_output_words(query_packet, &resp_packet, &error_code,
                                                       callback_functions_words);
                break;
            case 0x04:
                resp_length = handle_read_input_words(query_packet, &resp_packet, &error_code,
                                                      callback_functions_rowords);
                break;
            case 0x05:
                resp_length = handle_write_output_bit(query_packet, &resp_packet, &error_code,
                                                      callback_functions_rwbits, node);
                break;
            case 0x06:
                resp_length = handle_write_output_word(query_packet, &resp_packet, &error_code,
                                                       callback_functions_words, node);
                break;
            case 0x0F:
                resp_length = handle_write_output_bits(query_packet, &resp_packet, &error_code,
                                                       callback_functions_rwbits, node);
                break;
            case 0x10:
                resp_length = handle_write_output_words(query_packet, &resp_packet, &error_code,
                                                        callback_functions_words, node);
                break;
                /* return exception code 0x01 -> function not supported! */
            default:
                resp_length = -1;
                error_code = 0x01;
                break;
        } /* switch(function) */

        if (resp_length < 0) {
            /* return error... */
            /* build exception response frame... */
            resp_packet = resp_buffer_;
            resp_packet[0] = query_packet[0];        /* slave */
            resp_packet[1] = query_packet[1] | 0x80; /* function code with error bit activated! */
            resp_packet[2] = error_code;
            resp_length = 3;
        }
        modbus_write(entry, resp_packet, (u16) resp_length, transaction_id, 0 /*transmit_timeout*/, 0);
    } /* if not ignore request */
    /* humour the compiler... */
    return error_code;
}

#endif
